#include "slepdef.h"
#include "slepblas.h"
#include <math.h>
#include <assert.h>
#include "sleputil.h"
#include "slepmacros.h"

#ifdef max
	#undef max
	#define max(x,y) ((x)>(y)?(x):(y))
#endif


int LeastRTestImplementation(slep_matrix* A, slep_vector* y, double z, OPTS *opts, 
			slep_vector* x, slep_vector* funVal, slep_vector* valueL){

	static slep_vector *ATy , *Ax, *Axp, *xxp, *xp;
	static slep_vector *As, *ATAs, *Av, *Axy;
	static slep_vector *v, *s, *g;
	static int bFlag,i;
	static double L,lambda, l_sum, r_sum, beta, alpha, alphap;
	static int iterStep;
	assert( y->dim == A->m);

	ATy = slepAllocVector(A->n, SlepDense);
	ATAs = slepAllocVector(A->n, SlepDense);
	g = slepAllocVector(A->n, SlepDense);
	Ax = slepAllocVector(A->m, SlepDense);
	Axp = slepAllocVector(A->m, SlepDense);
	As = slepAllocVector(A->m, SlepDense);
	Av = slepAllocVector(A->m, SlepDense);
	Axy = slepAllocVector(A->m, SlepDense);
	xp = slepAllocVector(A->n, SlepDense);
	xxp = slepAllocVector(A->n, SlepDense);
	v = slepAllocVector(A->n, SlepDense);
	s = slepAllocVector(A->n, SlepDense);

	slep_daAxpby(SlepTranspose, 1.0 ,A,y,0.0 ,ATy);
	slep_daAxpby(SlepNoTranspose, 1., A,x,0.,Ax);

	if( opts->mFlag == 0 && opts->lFlag == 0){
		bFlag=0;
		L = 1 + opts->rsL2;
		slep_dvcopy(x,xp);
		slep_dvcopy(Ax,Axp);

		lambda=z;

		DMEMSET(xxp->data.vctr, 0, xxp->dim,i);
		alphap=0.;
		alpha=1.;

		for( iterStep=0; iterStep < opts->maxIter; iterStep++){
	
			beta = (alphap-1.)/alpha;
			slep_dvcopy(x,s);
			slep_daxpy(beta,xxp,s);

			slep_dvcopy(Ax,As);
			slep_dscal(1. +beta,As);
			slep_daxpy(-beta, Axp, As);

			slep_daAxpby(SlepTranspose,1. ,A,As,0. ,ATAs);
			slep_dvcopy(ATAs,g);
			slep_daxpy(-1.0,ATy,g);
			slep_daxpy(opts->rsL2,s,g);

			slep_dvcopy(x,xp);
			slep_dvcopy(Ax,Axp);

	
	

			for(;;){
				slep_dvcopy(s,v);
				slep_daxpy(-1/L,g,v);

				//x = sign(v).* max(abs(v) - lambda/L, 0); 
				if( v->sparsity == SlepSparse)
					for(i=0; i< v->data.spvctr.nnz; i++)
						v->data.spvctr.values[i] = 
							(v->data.spvctr.values[i]/fabs(v->data.spvctr.values[i]))
							* max(fabs(v->data.spvctr.values[i]) - lambda/L, 0);
				else
					for(i=0; i< v->dim; i++)
						v->data.vctr[i] = (v->data.vctr[i] / fabs(v->data.vctr[i]))
										* max( fabs(v->data.vctr[i]) - lambda/L, 0);


				slep_dvcopy(v,x);

				slep_daxpy(-1,s,v);

				slep_daAxpby(SlepNoTranspose, 1, A,x,0,Ax);

				slep_dvcopy(Ax,Av);
				slep_daxpy(-1,As,Av);
				r_sum = slep_ddot(v,v);
				l_sum = slep_ddot(Av,Av);

				if(r_sum <= 1e-20){
					bFlag=1;
					break;
				}

				if( l_sum <= r_sum * (L- opts->rsL2))
					break;
				else
					L = max( 2*L, opts->rsL2 + l_sum / r_sum );
				
			}

			valueL->data.vctr[iterStep] = L;
			alphap = alpha;
			alpha = (1+sqrt(4*alpha*alpha + 1))/2;

			slep_dvcopy(x,xxp);
			slep_daxpy(-1,xp,xxp);

			slep_dvcopy(Ax,Axy);
			slep_daxpy(-1,y,Axy);

			funVal->data.vctr[iterStep] = 0.5*(slep_ddot(Axy,Axy)+ opts->rsL2 * slep_ddot(x,x) + slep_dasum(x)*2*lambda );

			if(bFlag)
				break;

			if( iterStep >= opts->maxIter)
				break;

		}
	}

	slepFreeVector(ATy );
	slepFreeVector(ATAs );
	slepFreeVector(g );
	slepFreeVector(Ax);
	slepFreeVector(Axp);
	slepFreeVector(As);
	slepFreeVector(Av);
	slepFreeVector(Axy);
	slepFreeVector(xp );
	slepFreeVector(xxp );
	slepFreeVector(v );
	slepFreeVector(s );
	return 0;
}